#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>

#include "ohos_init.h"
#include "cmsis_os2.h"

#include "hi_types_base.h" 
#include "hi_errno.h"

#include "hi_io.h"
#include "hi_gpio.h"

#include "hi_i2c.h"

#include "hi_time.h"
#include "hi_watchdog.h"


#include "pca9685.h"
#include "uart_control.h"
extern unsigned char d_d;
extern unsigned char d_x;
extern unsigned char d_y;



#define SDA_IO_NAME     HI_IO_NAME_GPIO_0
#define SDA_IO_FUNC     HI_IO_FUNC_GPIO_0_I2C1_SDA

#define SCL_IO_NAME     HI_IO_NAME_GPIO_1
#define SCL_IO_FUNC     HI_IO_FUNC_GPIO_1_I2C1_SCL

#define I2C_IDX         HI_I2C_IDX_1
#define I2C_BAUDRATE    400000
#define PCA9685_PIN0    0  //上下
#define PCA9685_PIN1    1  //左右
#define PCA9685_PIN2    2
#define PCA9685_PIN3    3
 
hi_bool hx_state = HI_FALSE;    // 端口状态
hi_gpio_value hx_vol = 0;       // 端口电压值
hi_u8 hx_data = 0;              // 接收数据


static hi_u8 PCA9685_Control(hi_u8 con);

// I2C初始
hi_void I2C_Init(hi_void)
{
    hi_io_set_func(SDA_IO_NAME, SDA_IO_FUNC);
    hi_io_set_func(SCL_IO_NAME, SCL_IO_FUNC);
    if(hi_i2c_init(I2C_IDX, I2C_BAUDRATE) != 0)
    {
        printf(" [ I2C Init ] Failed! \n");
    }
}

static hi_u8 PCA9685_Control_1(hi_u8 con)
{
    PCA9685_Angle(PCA9685_PIN0, con);
    PCA9685_Set_PWM(PCA9685_PIN0, 0, 0);
    printf(" ang = 0 \n");
   
}
static hi_u8 PCA9685_Control_2(hi_u8 con)
{
    PCA9685_Angle(PCA9685_PIN1, con);
    PCA9685_Set_PWM(PCA9685_PIN1, 0, 0);
    printf(" ang = 0 \n");

}
static hi_u8 PCA9685_Reset_Init(hi_void)
{
    PCA9685_Set_PWM(PCA9685_PIN0, 0, 0);
    PCA9685_Set_PWM(PCA9685_PIN1, 0, 0);
    PCA9685_Set_PWM(PCA9685_PIN2, 0, 0);
    PCA9685_Set_PWM(PCA9685_PIN3, 0, 0); 
    PCA9685_Control_1(0x00);
    PCA9685_Control_2(0x00);
}

 
// 主任务
static void *Task(const char *arg)
{
    (void)arg;
    hi_u8 ang_x=90;
    hi_u8 ang_y=110;
    hi_u8 PCA_Flag=3;
    hi_u8 PCA_X=0;
    hi_u8 PCA_Y=0;
    hi_u8 PCA_X_Flag=0;
    hi_u8 PCA_Y_Flag=0;
    hi_u8 PCA_M=0;
    
    hi_watchdog_disable();//关闭开门狗
    hi_gpio_init();//初始化GPIPO
    I2C_Init(); //初始化IIC
    PCA9685_Init();//初始化PCA9685
    PCA9685_Set_PWM_Freq(50);//设置PCA9685频率
    PCA9685_Reset_Init();
    
    while(1)
    {
        printf(" d_d =%d \n",d_d);
        
        if(d_d==0)
        {
            printf(" [PCA_Flag=3 ] \n");
            PCA_Flag=3;
            if(PCA_X_Flag==1)
            {
                PCA_Flag=2;
            }
        }
        else if(d_d==1)
        {
            
            PCA_Flag=1;
            printf(" [PCA_Flag=1 ] \n");
        }
        
        if(PCA_Flag==1)
        {
            hi_u8 ang_x=0;
            hi_u8 ang_y=0;
            hi_u8 PCA_X=90;
            hi_u8 PCA_Y=110;
            printf(" PCA_ANG\n");
            printf(" d_x =%d \n",d_x);
            printf(" d_y =%d \n",d_y);
            if(d_x>128&&d_y>128)
            {
                PCA_X_Flag=(d_x-128)/4;
                PCA_Y_Flag=(d_y-128)/2;
                ang_x=PCA_X+PCA_X_Flag;
                
                ang_y=PCA_Y+PCA_Y_Flag;
                printf(" ang_x =%d \n",ang_x);
                printf(" ang_y =%d \n",ang_y);
            }
            if(d_x>128&&d_y<128)
            {
                PCA_X_Flag=(d_x-128)/4;
                PCA_Y_Flag=d_y/2;
                ang_x=PCA_X+PCA_X_Flag;
                ang_y=PCA_Y-PCA_Y_Flag;
                printf(" ang_x =%d \n",ang_x);
                printf(" ang_y =%d \n",ang_y);

            }
            if(d_x<128&&d_y>128)
            {
                PCA_X_Flag=d_x/4;
                PCA_Y_Flag=(d_y-128)/2;
                ang_x=PCA_X-PCA_X_Flag;
                ang_y=PCA_Y+PCA_Y_Flag;
                printf(" ang_x =%d \n",ang_x);
                printf(" ang_y =%d \n",ang_y);
            }
            if(d_x<128&&d_y<128)
            {
                PCA_X_Flag=d_x/4;
                PCA_Y_Flag=d_y/2;
                ang_x=PCA_X-PCA_X_Flag;
                ang_y=PCA_Y-PCA_Y_Flag;
                printf(" ang_x =%d \n",ang_x);
                printf(" ang_y =%d \n",ang_y);

            }   
            printf(" [ pca9685 t1 ] \n");
            
            
             PCA9685_Control_1(ang_x);
             printf(" ang_x =%d \n",ang_x);
             usleep(10000); // 休眠10ms
             usleep(10000); // 休眠10ms
             usleep(10000); // 休眠10ms
             usleep(10000); 
             
             PCA9685_Control_2(ang_y);
             printf(" ang_y =%d \n",ang_y);
             usleep(10000); // 休眠10ms
             usleep(10000); // 休眠10ms
             usleep(10000); // 休眠10ms
             usleep(10000); 
             
        }
        if(PCA_Flag==2)
        {
            printf(" [ pca9685 t2 ] \n"); 
            if(PCA_M==0)
            {
                ang_x++;  
            }
            else if(PCA_M==1)
            {
                ang_x--; 
            } 
            usleep(10000); // 休眠10ms
            usleep(10000); // 休眠10ms
            usleep(10000); // 休眠10ms
            usleep(10000);                              
            PCA9685_Control_1(ang_x);
            if(ang_x==130)
            {
                PCA_M=1;
            }
            else if(ang_x==60)
            {
                PCA_M=0;
            }
            usleep(10000); // 休眠10ms
            usleep(10000); // 休眠10ms
            usleep(10000); // 休眠10ms
            usleep(10000); // 休眠10ms
            PCA9685_Control_2(100);
        }
        if(PCA_Flag==3)
        { 
            printf(" [ pca9685 t3 ] \n");
             PCA9685_Control_1(90);
             usleep(100000); // 休眠10ms
             usleep(100000); // 休眠10ms
             usleep(100000); // 休眠10ms
             usleep(100000); 
             
             PCA9685_Control_2(90);
             usleep(100000); // 休眠10ms
             usleep(100000); // 休眠10ms
             usleep(100000); // 休眠10ms
             usleep(100000); 
             PCA_X_Flag=1;
        }      
    }
}


static void Entry(void)
{
    osThreadAttr_t attr;

    attr.name = "Task";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 2 * 1024;
    attr.priority = 25;
    printf("1111111");

    if(osThreadNew((osThreadFunc_t)Task, NULL, &attr) == NULL)
    {
        printf("Falied to create Task!\n");
    }
}

SYS_RUN(Entry);










